#include "driver_sg90.h"


static TIM_HandleTypeDef htim = {0};
static TIM_OC_InitTypeDef sConfig = {0};

void SG90_GPIO_Init(void)
{
    htim.Instance = TIM4;
    htim.Init.AutoReloadPreload = TIM_OUTPUTSTATE_DISABLE;
    htim.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim.Init.Period = 20000 - 1;
    htim.Init.Prescaler = 72 - 1;
    htim.Init.RepetitionCounter = 0;
    HAL_TIM_OC_Init(&htim); 

    sConfig.OCFastMode = TIM_OCFAST_ENABLE;
    sConfig.OCMode = TIM_OCMODE_PWM1;
    sConfig.OCPolarity = TIM_OCPOLARITY_HIGH;
    sConfig.Pulse = 0;
    HAL_TIM_OC_ConfigChannel(&htim, &sConfig, TIM_CHANNEL_2);
    HAL_TIM_OC_Start(&htim, TIM_CHANNEL_2);                                   
}


void HAL_TIM_OC_MspInit(TIM_HandleTypeDef * htim)
{
    if(htim->Instance == TIM4)
    {
        __HAL_RCC_TIM4_CLK_ENABLE();
        __HAL_RCC_GPIOB_CLK_ENABLE();
        
        GPIO_InitTypeDef GPIO_Init = {0};    
        GPIO_Init.Pin = SG90_Pin;
        GPIO_Init.Mode = GPIO_MODE_AF_PP;
        GPIO_Init.Pull = GPIO_PULLUP;
        GPIO_Init.Speed = GPIO_SPEED_FREQ_HIGH;     
        HAL_GPIO_Init(SG90_GPIO_Port, &GPIO_Init);   
    }
}

int Sg90SetStatus(struct Sg90Device *pDev, int status)
{
	if (pDev == NULL)
    {
		return PDFAIL;
    }
	if (status == SG90_FULL)
	{
		__HAL_TIM_SET_COMPARE(&htim, TIM_CHANNEL_2, 2500);
	}
	else if (status == SG90_HALF)
	{
		__HAL_TIM_SET_COMPARE(&htim, TIM_CHANNEL_2, 1500);
	}
    else if(status == SG90_CLOSE)
    {
        __HAL_TIM_SET_COMPARE(&htim, TIM_CHANNEL_2, 500);
    }
	return PDSUCCESS;	
}
